# euler_angles.py

r'''Calculates the euler angles for a given rotation matrix.

Uses rotating axes, with no axis repetition.  Thus, there are 6 possible
orders: XYZ, XZY, YXZ, YZX, ZXY, ZYX.

Taken from: *Euler Angle Conversion* by Ken Shoemake in *Graphics Gems IV*,
p 222-229, code: p 225-228.  See http://www.graphicsgems.org/.

Code from:
    http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/

Specifically, these three files:
    http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/QuatTypes.h
    http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.h
    http://tog.acm.org/resources/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
'''

from __future__ import division

import sys
import math

# Constants:
X = 0
Y = 1
Z = 2
EulParEven = 0
EulParOdd = 1

Parameters = {
    'XYZ': (Z, EulParOdd),
    'XZY': (Y, EulParEven),
    'YXZ': (Z, EulParEven),
    'YZX': (X, EulParOdd),
    'ZXY': (Y, EulParOdd),
    'ZYX': (X, EulParEven),
}

def EulNext(axis):
    return (1,2,0,1)[axis]

# In the original macro:
#    f is always EulFrmR
#    s is always EulRepNo
#    h (not used) is always == i
def EulGetOrd(order):
    r'''Returns i,j,k,n

    i,j,k are final_axis, 
    n is either EulParOdd or EulParEven.

    ``order`` is one of 6 3-char permutations of the letters: 'X', 'Y', and 'Z'.
    '''
    i, n = Parameters[order]
    return i, EulNext(i + n), EulNext(i + 1 - n), n

def Eul_FromHMatrix(M, order):
    r'''Returns x, y and z in degrees.

    ``M`` is a rotation matrix (could be 3x3 or 4x4).
    ``order`` is one of 6 3-char permutations of the letters: 'X', 'Y', and 'Z'.
    '''
    i,j,k,n = EulGetOrd(order)
    cy = math.hypot(M[i,i], M[j,i])

    # Original C code used FLT_EPSILON, this is DBL_EPSILON...
    if cy > 16 * sys.float_info.epsilon:
        x = math.atan2(M[k,j], M[k,k])
        y = math.atan2(-M[k,i], cy)
        z = math.atan2(M[j,i], M[i,i])
    else:
        x = math.atan2(-M[j,k], M[j,j])
        y = math.atan2(-M[k,i], cy)
        z = 0.0
    if n == EulParOdd:
        x = -x
        y = -y
        z = -z
    x, z = z, x
    return math.degrees(x), math.degrees(y), math.degrees(z)

